﻿
#include "KalmanFilter.h"
KalmanFilter::KalmanFilter(int stateNum, int measurementNum, int controlNum, float T)
{
    this->_stateNum = stateNum;
    this->_measureNum = measurementNum;
    this->_controlNum = controlNum;
    this->_T = T;

    _transitionMat = MatrixXd::Zero(_stateNum, _stateNum);
    _measureMat = MatrixXd::Zero(_measureNum, _stateNum);
    _controlMat = MatrixXd::Zero(_stateNum, _controlNum);
    _postCovMat = MatrixXd::Identity(_stateNum, _stateNum);
    _proNoiseCovMat = MatrixXd::Zero(_stateNum, _stateNum);
    _meaNoiseCovMat = MatrixXd::Zero(_measureNum, _measureNum);

    _stateVec = VectorXd::Zero(_stateNum);
    _controlVec = VectorXd::Zero(_controlNum);
    _processNoise = VectorXd::Zero(_stateNum);
    _measureNoise = VectorXd::Zero(_measureNum);
}

KalmanFilter::~KalmanFilter() {}

MatrixXd KalmanFilter::predict()
{
    if (_controlNum == 0) {
        this->_stateVec = this->_transitionMat * this->_stateVec;
    } else {
        this->_stateVec =
            this->_transitionMat * this->_stateVec + this->_controlMat * this->_controlVec;
    }
    this->_postCovMat =
        this->_transitionMat * this->_postCovMat * this->_transitionMat.transpose() +
        this->_proNoiseCovMat;
    return this->_stateVec;
}

MatrixXd KalmanFilter::correct(const VectorXd& measureVec)
{
    if (measureVec.size() != _measureNum) {
        std::cerr << "Wrong with dimension of measurement vector!" << std::endl;
        return {};
    }
    // 计算kalman增益, n*p的矩阵
    MatrixXd kt = this->_postCovMat * this->_measureMat.transpose() *
                  (this->_measureMat * this->_postCovMat * _measureMat.transpose() +
                   this->_meaNoiseCovMat)
                      .inverse();
    // 计算最优估计
    _stateVec = _stateVec + kt * (measureVec - _measureMat * _stateVec);
    // 更新后验状态估计协方差
    _postCovMat = (MatrixXd::Identity(_stateNum, _stateNum) - kt * _measureMat) * _postCovMat;
    return _stateVec;
}

bool KalmanFilter::setTansMat(const MatrixXd& transitionMat)
{
    _transitionMat = transitionMat;
    return true;
}
bool KalmanFilter::setMeasMat(const MatrixXd& measurementnMat)
{
    _measureMat = measurementnMat;
    return true;
}
bool KalmanFilter::setQ(const MatrixXd& proNoiseMat)
{
    _proNoiseCovMat = proNoiseMat;
    return true;
}
bool KalmanFilter::setR(const MatrixXd& meaNoiseCovMat)
{
    _meaNoiseCovMat = meaNoiseCovMat;
    return true;
}

void KalmanFilter::info()
{
    std::cout << " ddddd" << std::endl;
    std::cout << MatrixXd::Identity(_stateNum, _stateNum) << std::endl;

    std::cout << " ddddd" << std::endl;
}
